/**
 Copyright will be fixed soon.
 Author: Hiroshi Ota.
 $Date: $
 $Revision: $
*/

/**
 Copyright will be fixed soon.
 Author: Hiroshi Ota.
 $Date: $
 $Revision: $
*/

#include "BFBehaviorControl.h"
#include "BFBehaviorControlLogMessage.h"

// Variation
// #define _NOT_USE_LOGGING_IN_RUNNING

BFBehaviorControl::BFBehaviorControl(Motor* drive, Motor* stairing,
		LightSensor* light,
#ifndef _NO_USE_BF_TOUCH_SENSOR
		TouchSensor* touch,
#endif
		IBFUI* ui, IBFLog* log)
: currentState(0), initApp(0), runApp(0), termApp(0),
	driveMotor(drive), stairingMotor(stairing),
	lightSensor(light),
#ifndef _NO_USE_BF_TOUCH_SENSOR
	touchSensor(touch),
#endif
	uiModule(ui), logModule(log)
{
	currentState = new BFState();
}

BFBehaviorControl::~BFBehaviorControl(void)
{
	delete currentState;
}

void BFBehaviorControl::registState(BFState *state)
{
	if (currentState==0) {
		currentState = state;
	} else {
		if (currentState!=state) {
			delete currentState;
			currentState = state;
		}
	}
}

void BFBehaviorControl::updateState(BFState::SystemState newState)
{
	currentState->setSystemState(newState);
}

int BFBehaviorControl::start()
{
	if (initApp==0||runApp==0||termApp==0||currentState==0) {
		return -1;
	}

	// State is Initialization
	int result = initApp->execEntry(currentState);
	currentState->setSystemState(BFState::SS_INITIALIZATION);
	result = initApp->execDo(currentState);
	result = initApp->execExit(currentState);
	if (result==0) {
		logModule->log(currentState,
			BF_BEHAVIOR_CONTROL_LOGCODE,
			BF_BEHAVIOR_CONTROL_LOGCODE_INIT_SUCCEEDED);
	}
	if (result==-1) {
		return -1;
	}
	// State is Running
	result = runApp->execEntry(currentState);
	currentState->setSystemState(BFState::SS_RUNNING);
	result = runApp->execDo(currentState);
	result = runApp->execExit(currentState);
	if (result==0) {
		logModule->log(currentState,
			BF_BEHAVIOR_CONTROL_LOGCODE,
			BF_BEHAVIOR_CONTROL_LOGCODE_RUNNING_SUCCEEDED);
	}

	// State is Terminating
	result = termApp->execEntry(currentState);
	currentState->setSystemState(BFState::SS_TERMINATION);
	result = termApp->execDo(currentState);
	result = termApp->execExit(currentState);
	if (result==0) {
		logModule->log(currentState,
			BF_BEHAVIOR_CONTROL_LOGCODE,
			BF_BEHAVIOR_CONTROL_LOGCODE_TERM_SUCCEEDED);
	}

	return result;
}
